/* -------------------------------------------------------------------------- *
 *                         OpenSim:  testMuscles.cpp                          *
 * -------------------------------------------------------------------------- *
 * The OpenSim API is a toolkit for musculoskeletal modeling and simulation.  *
 * See http://opensim.stanford.edu and the NOTICE file for more information.  *
 * OpenSim is developed at Stanford University and supported by the US        *
 * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA    *
 * through the Warrior Web program.                                           *
 *                                                                            *
 * Copyright (c) 2005-2017 Stanford University and the Authors                *
 * Author(s): Ajay Seth, Matthew Millard                                      *
 *                                                                            *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
 * not use this file except in compliance with the License. You may obtain a  *
 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
 *                                                                            *
 * Unless required by applicable law or agreed to in writing, software        *
 * distributed under the License is distributed on an "AS IS" BASIS,          *
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
 * See the License for the specific language governing permissions and        *
 * limitations under the License.                                             *
 * -------------------------------------------------------------------------- */

//=============================================================================
//  testMuscles simulates OpenSim Muscles using the OpenSim API and 
//  tests that muscles can compute equilibrium under varying conditions, 
//  including the full range of activations. Simulations are also tested
//  for energy conservation by comparing changes in system power to muscle
//  power.
//
//  Models tested include:
//      1. RigidTendonMuscle (Stateless, with user-defined f-l, f-v splines)
//      2. Thelen2003Muscle (Uses the Muscle interface)
//      3. Millard2012EquilibriumMuscle
//      4. Millard2012AccelerationMuscle
//      5. DeGrooteFregly2016Muscle
//      
//     Add more test cases to address specific problems with muscle models
//
//==============================================================================
#include <OpenSim/Common/osimCommon.h>
#include <OpenSim/Simulation/osimSimulation.h>
#include <OpenSim/Actuators/osimActuators.h>
#include <OpenSim/Analyses/osimAnalyses.h>
#include <OpenSim/Auxiliary/auxiliaryTestFunctions.h>
#include <OpenSim/Auxiliary/auxiliaryTestMuscleFunctions.h>

#include <catch2/catch_all.hpp>

using namespace OpenSim;
using namespace std;

//==============================================================================
static const double IntegrationAccuracy         = 1e-5;
static const double SimulationTestTolerance     = 1e-4;
static const double InitializationTestTolerance = 1e-6;
static const double CorrectnessTestTolerance    = 1e-6;

static const int SimulationTest         = 0;
static const int InitializationTest     = 1;
static const int CorrectnessTest        = 2;

// MUSCLE CONSTANTS
static const double MaxIsometricForce0  = 100.0, 
                    OptimalFiberLength0 = 0.1, 
                    TendonSlackLength0  = 0.2, 
                    PennationAngle0     = 0.0,
                    PennationAngle1     = SimTK::Pi/4;

static const double Activation0     = 0.01, 
                    Deactivation0   = 0.4,  
                    ShutteDelpActivation1 = 7.6,    
                    ShutteDelpActivation2 = 2.5;

namespace {
    /**
    This function is a simulateMuscle helper that tests for muscle equilibrium,
    i.e. that the muscle and tendon forces are equal at every state. Muscle
    models that are not equilibrium-based are ignored.

    @param model    the model with the muscles to test
    @param statesStore  the set of states
    */
    void testMuscleEquilibriumSolve(const Model& model, const Storage& statesStore)
    {
        // Get the muscle to test
        const Muscle& muscle = model.getMuscles()[0];

        if (dynamic_cast<const Millard2012AccelerationMuscle*>(&muscle)) {
            // Millard2012AccelerationMuscle is not an Equilibrium-based Muscle
            return;
        }

        // Load input data as StatesTrajectory used to perform the Analysis
        auto statesTraj = StatesTrajectory::createFromStatesStorage(
            model, statesStore, true, false);
        size_t nstates = statesTraj.getSize();

        // muscle active, passive, total muscle and tendon force quantities
        double af, pf, mf, tf, nfl, fv;
        af = pf = mf = tf = nfl = fv = SimTK::NaN;

        // Tolerance for muscle equilibrium solution
        const double equilTol = muscle.getMaxIsometricForce()*SimTK::SqrtEps;

        // The maximum acceptable change in force between two contiguous states
        const double maxDelta = muscle.getMaxIsometricForce() / 10;

        const int N = 20;
        const double minActivation = muscle.getMinControl();
        const double dAct = (1.0 - minActivation) / N;
        double activation = 0;

        SimTK::State s = model.getWorkingState();
        // Independently compute the active fiber force at every state
        for (size_t i = 0; i < nstates; ++i) {
            s = statesTraj[i];

            // test a full sweep of default activations at each state
            for (int j = 0; j <= N; ++j) {
                activation = minActivation + j*dAct;
                muscle.setActivation(s, activation);

                try {
                    muscle.computeEquilibrium(s);
                }
                catch (const MuscleCannotEquilibrate&) {
                    // Write out the muscle equilibrium error as a function of
                    // fiber lengths.
                    if (const auto* thelen =
                        dynamic_cast<const Thelen2003Muscle*>(&muscle)) {
                        thelen->printCurveToCSVFile(
                            Thelen2003Muscle::CurveType::FiberForceVelocity, "");
                        reportTendonAndFiberForcesAcrossFiberLengths(*thelen, s);
                        }
                    else if (const auto* millard =
                        dynamic_cast<const Millard2012EquilibriumMuscle*>(&muscle)) {
                        reportTendonAndFiberForcesAcrossFiberLengths(*millard, s);
                        }

                    throw;
                }

                model.realizeDynamics(s);

                // Get the fiber-length
                nfl = muscle.getNormalizedFiberLength(s);

                SimTK_ASSERT_ALWAYS(nfl >= 0.0,
                    "Equilibrium failed to compute valid fiber length.");

                // get active and passive forces given the default activation
                af = muscle.getActiveFiberForceAlongTendon(s);
                pf = muscle.getPassiveFiberForceAlongTendon(s);
                // now the total muscle force is the active + passive
                mf = af + pf;
                tf = muscle.getTendonForce(s);

                // equilibrium demands tendon and muscle fiber forces are equivalent
                ASSERT_EQUAL<double>(tf, mf, equilTol,
                    __FILE__, __LINE__, "testMuscleEquilibriumSolve(): " +
                    muscle.getConcreteClassName() +
                    " failed to solve for muscle (fiber) and tendon equilibrium. ");
            }
        }
    }


    /**
    Main test driver to be used on any muscle model (derived from Muscle) so new
    cases should be easy to add. This function completes a controlled activation,
    controlled stretch simulation of a muscle. After the simulation has completed,
    the results can be tested in a number of different ways to ensure that the
    muscle model is functioning. Currently, the test only verifies that the work
    done by the muscle corresponds to the change in system energy.

    @param aMuscModel  a path actuator
    @param startX   the starting position of the muscle anchor. I have no idea
                    why this value is included.
    @param act0     the initial activation of the muscle
    @param motion   the forced stretch of the simulation
    @param control  the activation control signal that is applied to the muscle
    @param printResults print the osim model associated with this test.
    */
    void simulateMuscle(
            const Muscle &aMuscModel,
            double startX,
            double act0,
            const Function *motion,  // prescribe motion of free end of muscle
            const Function *control, // prescribed excitation signal to the muscle
            bool printResults)
    {
        string prescribed = (motion == NULL) ? "." : " with Prescribed Motion.";

        cout << "\n******************************************************" << endl;
        cout << "Test " << aMuscModel.getConcreteClassName()
             << " Model" << prescribed << endl;
        cout << "******************************************************" << endl;
        using SimTK::Vec3;

        //==========================================================================
        // 0. SIMULATION SETUP: Create the block and ground
        //==========================================================================

        // Define the initial and final simulation times
        double initialTime = 0.0;
        double finalTime = 0.5;

        //Physical properties of the model
        double ballMass = 10;
        double ballRadius = 0.05;
        double anchorWidth = 0.1;

        // Create an OpenSim model
        Model model;

        double optimalFiberLength = aMuscModel.getOptimalFiberLength();
        double pennationAngle     = aMuscModel.getPennationAngleAtOptimalFiberLength();
        double tendonSlackLength  = aMuscModel.getTendonSlackLength();

        // Use a copy of the muscle model passed in to add path points later
        PathActuator *aMuscle = aMuscModel.clone();
        *aMuscle = aMuscModel;

        // Get a reference to the model's ground body
        Ground& ground = model.updGround();
        //ground.updDisplayer()->setScaleFactors(Vec3(anchorWidth, anchorWidth, 2*anchorWidth));

        OpenSim::Body * ball = new OpenSim::Body("ball",
                            ballMass ,
                            Vec3(0),
                            ballMass*SimTK::Inertia::sphere(ballRadius));

        ball->attachGeometry(new Sphere(ballRadius));
        //ball->updDisplayer()->setScaleFactors(Vec3(2*ballRadius));
        // ball connected  to ground via a slider along X
        double xSinG = optimalFiberLength*cos(pennationAngle)+tendonSlackLength;

        SliderJoint* slider = new SliderJoint( "slider",
                            ground,
                            Vec3(anchorWidth/2+xSinG, 0, 0),
                            Vec3(0),
                            *ball,
                            Vec3(0),
                            Vec3(0));

        auto& sliderCoord = slider->updCoordinate();
        sliderCoord.setName("tx");
        sliderCoord.setDefaultValue(1.0);
        sliderCoord.setRangeMin(0);
        sliderCoord.setRangeMax(1.0);

        if(motion != NULL){
            sliderCoord.setPrescribedFunction(*motion);
            sliderCoord.setDefaultIsPrescribed(true);
        }
        // add ball to model
        model.addBody(ball);
        model.addJoint(slider);

        //==========================================================================
        // 1. SIMULATION SETUP: Add the muscle
        //==========================================================================

        //Attach the muscle
        const string &actuatorType = aMuscle->getConcreteClassName();
        aMuscle->setName("muscle");
        aMuscle->addNewPathPoint("muscle-box", ground, Vec3(anchorWidth/2,0,0));
        aMuscle->addNewPathPoint("muscle-ball", *ball, Vec3(-ballRadius,0,0));

        ActivationFiberLengthMuscle_Deprecated *aflMuscle
            = dynamic_cast<ActivationFiberLengthMuscle_Deprecated *>(aMuscle);
        if(aflMuscle){
            // Define the default states for the muscle that has
            //activation and fiber-length states
            aflMuscle->setDefaultActivation(act0);
            aflMuscle->setDefaultFiberLength(aflMuscle->getOptimalFiberLength());
        }else{
            ActivationFiberLengthMuscle *aflMuscle2
                = dynamic_cast<ActivationFiberLengthMuscle *>(aMuscle);
            if(aflMuscle2){
                // Define the default states for the muscle
                //that has activation and fiber-length states
                aflMuscle2->setDefaultActivation(act0);
                aflMuscle2->setDefaultFiberLength(aflMuscle2
                    ->getOptimalFiberLength());
            }
        }

        model.addForce(aMuscle);

        // Create a prescribed controller that simply
        //applies controls as function of time
        std::unique_ptr<PrescribedController>
            muscleController{new PrescribedController{}};
        if(control != NULL){
            muscleController->setActuators(model.updActuators());
            // Set the individual muscle control functions
            //for the prescribed muscle controller
            muscleController->prescribeControlForActuator("muscle",
                    *control);

            // Add the control set controller to the model
            model.addController(muscleController.release());
        }

        // Set names for muscles / joints.
        Array<string> muscNames;
        muscNames.append(aMuscle->getName());
        Array<string> jointNames;
        jointNames.append("slider");

        //==========================================================================
        // 2. SIMULATION SETUP: Instrument the test with probes
        //==========================================================================

        // Add an ActuatorPowerProbe to measure the work done by the muscle actuator
        ActuatorPowerProbe * muscWorkProbe =
            new ActuatorPowerProbe(muscNames, true, 1);
        muscWorkProbe->setOperation("integrate");
        model.addProbe(muscWorkProbe);

        // Add a JointInternalPowerProbe to measure the work done by the joint
        // will be 0 unless joint has prescribed motion
        JointInternalPowerProbe * jointWorkProbe =
            new JointInternalPowerProbe(jointNames, true, 1);
        jointWorkProbe->setOperation("integrate");
        model.addProbe(jointWorkProbe);

        model.finalizeConnections(); // Needed so sockets have correct absolute path on print
        /* Since all components are allocated on the stack don't have model
           own them (and try to free)*/
        //  model.disownAllComponents();
        model.setName(actuatorType+"ModelTest");
        model.print(actuatorType+"ModelTest.osim");

        /* Setup a Muscle Analysis to report all internal values of the
           muscle during the simulation. If you uncomment, remember to
           uncomment the corresponding calls to write the results to
           file after the simulation.*/
        MuscleAnalysis * muscleAnalysis = new MuscleAnalysis();
        Array<string> tmp;
        tmp.append("muscle");
        muscleAnalysis->setMuscles(tmp);
        model.addAnalysis(muscleAnalysis);

        /*
            Muscle *musclePtr = dynamic_cast<Muscle *>(aMuscle);
            MuscleActiveFiberPowerProbe fiberPowerProbe(&musclePtr);
            fiberPowerProbe.setName("ActiveFiberPower");
            fiberPowerProbe.setOperation("integrate");
            fiberPowerProbe.setOperationParameter(0);

            if(musclePtr){
                model.addProbe(&fiberPowerProbe);
            }
        */

        // Define visualizer for debugging
        //model.setUseVisualizer(true);

        //==========================================================================
        // 3. SIMULATION Initialization
        //==========================================================================

        // Initialize the system and get the default state
        SimTK::State& si = model.initSystem();
        model.getMultibodySystem().realize(si,SimTK::Stage::Dynamics);
        model.equilibrateMuscles(si);

        CoordinateSet& modelCoordinateSet = model.updCoordinateSet();

        // Define non-zero (defaults are 0) states for the free joint
        // set x-translation value
        modelCoordinateSet[0].setValue(si, startX, true);

        //Copy the initial state
        SimTK::State initialState(si);

        // Check muscle is setup correctly
        const PathActuator &actu
            = dynamic_cast<const PathActuator&>(model.updActuators().get("muscle"));
        double length = actu.getLength(si);
        double trueLength = startX + xSinG - anchorWidth/2;

        ASSERT_EQUAL(length/trueLength, 1.0, InitializationTestTolerance,
                    __FILE__, __LINE__,
                    "testMuscles: path failed to initialize to correct length." );

        model.getMultibodySystem().realize(si, SimTK::Stage::Acceleration);

        double Emuscle0 = muscWorkProbe->getProbeOutputs(si)(0);
        INFO("Muscle initial energy = " << Emuscle0);
        double Esys0 = model.getMultibodySystem().calcEnergy(si);
        Esys0 += (Emuscle0 + jointWorkProbe->getProbeOutputs(si)(0));
        double PEsys0 = model.getMultibodySystem().calcPotentialEnergy(si);
        INFO("Total initial system energy = " << Esys0);
        INFO("System potential energy = " << PEsys0);

        //==========================================================================
        // 4. SIMULATION Integration
        //==========================================================================

        // Create the manager
        Manager manager(model);
        manager.setIntegratorAccuracy(IntegrationAccuracy);

        // Integrate from initial time to final time
        si.setTime(initialTime);
        manager.initialize(si);
        cout<<"\nIntegrating from " << initialTime<< " to " << finalTime << endl;

        // Start timing the simulation
        const clock_t start = clock();
        // simulate
        manager.integrate(finalTime);

        // how long did it take?
        double comp_time = (double)(clock()-start)/CLOCKS_PER_SEC;

        // Save the simulation results
        Storage states(manager.getStateStorage());
        states.print(actuatorType+"_states.sto");

        StatesTrajectory statesTraj =
            StatesTrajectory::createFromStatesStorage(model, states);

        //==========================================================================
        // 5. SIMULATION Reporting
        //==========================================================================

        double realTimeMultiplier = ((finalTime-initialTime)/comp_time);
        printf("testMuscles: Realtime Multiplier: %f\n"
               "           :  simulation duration / clock duration\n"
               "              > 1 : faster than real time\n"
               "              = 1 : real time\n"
               "              < 1 : slower than real time\n",
                realTimeMultiplier );

        /*
        ASSERT(comp_time <= (finalTime-initialTime));
        printf("testMuscles: PASSED Realtime test\n"
               "             %s simulation time: %f with accuracy %f\n\n",
                             actuatorType.c_str(), comp_time , accuracy);
        */

        //An analysis only writes to a dir that exists, so create here.
        if(printResults){
            IO::makeDir("testMuscleResults");
            muscleAnalysis->printResults(actuatorType, "testMuscleResults");
        }

        double muscleWork = muscWorkProbe->getProbeOutputs(si)(0);
        cout << "Muscle work = " << muscleWork << endl;

        //==========================================================================
        // 6. SIMULATION Tests
        //==========================================================================
        auto& muscle = model.updMuscles()[0];
        testMuscleEquilibriumSolve(model, states);

        /*==========================================================================
            7. Correctness test:  d/dt(KE+PE-W) = 0 ?

            Check that the derivative of system energy less work is conserved
        ============================================================================
        */
        if(true){
            Storage* fiberActivePwrSto
                = muscleAnalysis->getFiberActivePowerStorage();
            Storage* fiberPassivePwrSto
                = muscleAnalysis->getFiberPassivePowerStorage();
            Storage* tendonPwrSto
                = muscleAnalysis->getTendonPowerStorage();
            Storage* musclePwrSto
                = muscleAnalysis->getMusclePowerStorage();


            double *fiberActivePwrDat  = NULL;
            double *fiberPassivePwrDat = NULL;
            double *tendonPwrDat       = NULL;
            double *musclePwrDat       = NULL;

            fiberActivePwrSto->getDataColumn(   "#1",fiberActivePwrDat);
            fiberPassivePwrSto->getDataColumn(  "#1",fiberPassivePwrDat);
            tendonPwrSto->getDataColumn("#1", tendonPwrDat);
            musclePwrSto->getDataColumn("#1",musclePwrDat);
            std::unique_ptr<double[]>  fiberActivePwrDat_ptr{fiberActivePwrDat};
            std::unique_ptr<double[]> fiberPassivePwrDat_ptr{fiberPassivePwrDat};
            std::unique_ptr<double[]>       tendonPwrDat_ptr{tendonPwrDat};
            std::unique_ptr<double[]>       musclePwrDat_ptr{musclePwrDat};

            double dKEPEW_dt = 0;

            double dtendonPE    = 0;
            double dfiberPE     = 0;
            double dfiberW      = 0;
            double dboundaryW   = 0;

            int numSteps = fiberActivePwrSto->getSize();

            for(int i=0; i<numSteps; i++){
                dtendonPE   = -tendonPwrDat[i];
                dfiberPE    = -fiberPassivePwrDat[i];
                dfiberW     =  fiberActivePwrDat[i];
                dboundaryW  =  -musclePwrDat[i];

                dKEPEW_dt = dtendonPE + dfiberPE - dfiberW - dboundaryW;

                ASSERT_EQUAL( dKEPEW_dt, 0.0, CorrectnessTestTolerance,
                              __FILE__, __LINE__,
                            "testMuscles: d/dt(system energy-work) non-zero.");
            }
        }
    }
}

//==============================================================================
// Individual muscle model (derived from Muscle) test cases can be added here
//==============================================================================

TEST_CASE("testRigidTendonMuscle") {
    RigidTendonMuscle   muscle( "muscle",
                                MaxIsometricForce0,
                                OptimalFiberLength0,
                                TendonSlackLength0,
                                PennationAngle0);

    double x0 = 0;
    double act0 = 0.5;
    Constant control(act0);

    Sine motion(OptimalFiberLength0, 2.0*SimTK::Pi, 0);

    // concentric
    simulateMuscle(muscle, 
        x0, 
        act0, 
        NULL, 
        &control, 
        false);

    // eccentric 
    simulateMuscle(muscle, 
        x0, 
        act0, 
        &motion,
        &control, 
        false);
}

TEST_CASE("testThelen2003Muscle")
{

    Thelen2003Muscle muscle("muscle",
                            MaxIsometricForce0,
                            OptimalFiberLength0,
                            TendonSlackLength0,
                            PennationAngle0);

    Thelen2003Muscle muscle1("muscle",
                            MaxIsometricForce0,
                            OptimalFiberLength0,
                            TendonSlackLength0,
                            PennationAngle1);

    muscle.setActivationTimeConstant(Activation0);
    muscle.setDeactivationTimeConstant(Deactivation0);

    double x0 = 0;
    double act0 = 0.2;

    Constant control(0.5);

    Sine motion(OptimalFiberLength0, 2.0*SimTK::Pi, 0);


    simulateMuscle(muscle, 
        x0, 
        act0, 
        &motion, 
        &control, 
        false);

    simulateMuscle(muscle1, 
        x0, 
        act0, 
        &motion, 
        &control, 
        false);

    Model m;
    muscle.addNewPathPoint("p1", m.getGround(), SimTK::Vec3(0.0));
    muscle.addNewPathPoint("p2", m.getGround(), SimTK::Vec3(1.0));
    // Test property bounds.
    {
        Thelen2003Muscle musc = muscle;
        musc.set_FmaxTendonStrain(0.0);
        SimTK_TEST_MUST_THROW_EXC(musc.finalizeFromProperties(),
                SimTK::Exception::ErrorCheck);

        musc.set_FmaxTendonStrain(0.1);
        musc.finalizeFromProperties();
    }
    {
        Thelen2003Muscle musc = muscle;
        musc.set_FmaxMuscleStrain(0.0);
        SimTK_TEST_MUST_THROW_EXC(musc.finalizeFromProperties(),
                SimTK::Exception::ErrorCheck);
    }
    {
        Thelen2003Muscle musc = muscle;
        musc.set_KshapeActive(0.0);
        SimTK_TEST_MUST_THROW_EXC(musc.finalizeFromProperties(),
                SimTK::Exception::ErrorCheck);
    }
    {
        Thelen2003Muscle musc = muscle;
        musc.set_KshapePassive(0.0);
        SimTK_TEST_MUST_THROW_EXC(musc.finalizeFromProperties(),
                SimTK::Exception::ErrorCheck);
    }
    {
        Thelen2003Muscle musc = muscle;
        musc.set_Af(0.0);
        SimTK_TEST_MUST_THROW_EXC(musc.finalizeFromProperties(),
                SimTK::Exception::ErrorCheck);
    }
    {
        Thelen2003Muscle musc = muscle;
        musc.set_Flen(1.001);
        musc.set_fv_linear_extrap_threshold(5.0);
        musc.finalizeFromProperties();

        musc.set_Flen(1.0);
        SimTK_TEST_MUST_THROW_EXC(musc.finalizeFromProperties(),
                SimTK::Exception::ErrorCheck);
    }
    {
        Thelen2003Muscle musc = muscle;
        musc.set_Flen(1.3);
        musc.finalizeFromProperties();
    }
    {
        Thelen2003Muscle musc = muscle;
        musc.set_fv_linear_extrap_threshold(1.0 / 1.4);
        SimTK_TEST_MUST_THROW_EXC(musc.finalizeFromProperties(),
                SimTK::Exception::ErrorCheck);

        musc.set_fv_linear_extrap_threshold(1.007);
        musc.finalizeFromProperties();

        musc.set_Flen(3.0);

        musc.set_fv_linear_extrap_threshold(1.0 / 3.0);
        SimTK_TEST_MUST_THROW_EXC(musc.finalizeFromProperties(),
                SimTK::Exception::ErrorCheck);

        musc.set_fv_linear_extrap_threshold(1.001 / 3.0);
        musc.finalizeFromProperties();
    }

    // Ensure the properties of MuscleFixedWidthPennationModel and
    // MuscleFirstOrderActivationDynamicModel are being set by Thelen2003Muscle
    // when they are subcomponents of the Muscle. Here, we set the properties
    // of Thelen2003Muscle and then check the properties of its subcomponents.
    // Also ensures properties survive serialization/deserialization.
    {
        const string& filename = "testSettingMuscleSubcomponentProperties.osim";
        const double optimalFiberLength = 1234.56;
        const double pennAngAtOptimal   = 0.123456;
        const double maximumPennation   = 1.23456;
        const double actTimeConstant    = 0.0123;
        const double deactTimeConstant  = 0.0246;
        const double minimumActivation  = 0.0357;

        // Create muscle and add to model.
        Model myModel;
        cout << "new Thelen2003Muscle myMuscle" << endl;
        Thelen2003Muscle* myMcl = new Thelen2003Muscle("myMuscle",
            MaxIsometricForce0, OptimalFiberLength0, TendonSlackLength0,
            PennationAngle0);
        
        myMcl->addNewPathPoint("p1", myModel.getGround(), SimTK::Vec3(0.0));
        myMcl->addNewPathPoint("p2", myModel.getGround(), SimTK::Vec3(1.0));
        myModel.addForce(myMcl);

        // Set properties of Thelen2003Muscle.
        myMcl->setOptimalFiberLength(optimalFiberLength);
        myMcl->setPennationAngleAtOptimalFiberLength(pennAngAtOptimal);
        myMcl->setMaximumPennationAngle(maximumPennation);
        myMcl->setActivationTimeConstant(actTimeConstant);
        myMcl->setDeactivationTimeConstant(deactTimeConstant);
        myMcl->setMinimumActivation(minimumActivation);
        myMcl->setMinControl(minimumActivation);

        cout << "myMuscle->finalizeFromProperties" << endl;
        myMcl->finalizeFromProperties();

        // Check properties of MuscleFixedWidthPennationModel.
        const MuscleFixedWidthPennationModel& pennMdl =
            myMcl->getPennationModel();
        ASSERT_EQUAL(optimalFiberLength, pennMdl.get_optimal_fiber_length(),
            SimTK::SignificantReal, __FILE__, __LINE__,
            "optimal_fiber_length was not set in pennation model");
        ASSERT_EQUAL(pennAngAtOptimal, pennMdl.get_pennation_angle_at_optimal(),
            SimTK::SignificantReal, __FILE__, __LINE__,
            "pennation_angle_at_optimal was not set in pennation model");
        ASSERT_EQUAL(maximumPennation, pennMdl.get_maximum_pennation_angle(),
            SimTK::SignificantReal, __FILE__, __LINE__,
            "maximum_pennation_angle was not set in pennation model");

        // Check properties of MuscleFirstOrderActivationDynamicModel.
        const MuscleFirstOrderActivationDynamicModel& actMdl =
            myMcl->getActivationModel();
        ASSERT_EQUAL(actTimeConstant, actMdl.get_activation_time_constant(),
            SimTK::SignificantReal, __FILE__, __LINE__,
            "activation_time_constant was not set in activation model");
        ASSERT_EQUAL(deactTimeConstant, actMdl.get_deactivation_time_constant(),
            SimTK::SignificantReal, __FILE__, __LINE__,
            "deactivation_time_constant was not set in activation model");
        ASSERT_EQUAL(minimumActivation, actMdl.get_minimum_activation(),
            SimTK::SignificantReal, __FILE__, __LINE__,
            "minimum_activation was not set in activation model");

        myModel.finalizeConnections();  // Needed so sockets have correct absolute path on print
        // Print model and read back in.
        myModel.print(filename);
        Model myModel2(filename);

        const Thelen2003Muscle& myMcl2 = dynamic_cast<const Thelen2003Muscle&>(
            myModel2.getMuscles().get("myMuscle") );

        // Check properties of MuscleFixedWidthPennationModel.
        const MuscleFixedWidthPennationModel& pennMdl2 =
            myMcl2.getPennationModel();
        ASSERT_EQUAL(optimalFiberLength, pennMdl2.get_optimal_fiber_length(),
            SimTK::SignificantReal, __FILE__, __LINE__,
            "optimal_fiber_length was not set in pennation model");
        ASSERT_EQUAL(pennAngAtOptimal, pennMdl2.get_pennation_angle_at_optimal(),
            SimTK::SignificantReal, __FILE__, __LINE__,
            "pennation_angle_at_optimal was not set in pennation model");
        ASSERT_EQUAL(maximumPennation, pennMdl2.get_maximum_pennation_angle(),
            SimTK::SignificantReal, __FILE__, __LINE__,
            "maximum_pennation_angle was not set in pennation model");

        // Check properties of MuscleFirstOrderActivationDynamicModel.
        const MuscleFirstOrderActivationDynamicModel& actMdl2 =
            myMcl2.getActivationModel();
        ASSERT_EQUAL(actTimeConstant, actMdl2.get_activation_time_constant(),
            SimTK::SignificantReal, __FILE__, __LINE__,
            "activation_time_constant was not set in activation model");
        ASSERT_EQUAL(deactTimeConstant, actMdl2.get_deactivation_time_constant(),
            SimTK::SignificantReal, __FILE__, __LINE__,
            "deactivation_time_constant was not set in activation model");
        ASSERT_EQUAL(minimumActivation, actMdl2.get_minimum_activation(),
            SimTK::SignificantReal, __FILE__, __LINE__,
            "minimum_activation was not set in activation model");
    }

    // Test exception when muscle cannot be initialized.
    {
        Model model;

        const double optimalFiberLength = 0.001; //short fiber and tendon
        const double tendonSlackLength  = 0.001;
        auto muscle = new Thelen2003Muscle("muscle", 1., optimalFiberLength,
                                           tendonSlackLength, 0.);
        muscle->addNewPathPoint("p1", model.updGround(), SimTK::Vec3(0));
        muscle->addNewPathPoint("p2", model.updGround(), SimTK::Vec3(0,0,1));
        model.addForce(muscle);

        SimTK::State& state = model.initSystem();
        ASSERT_THROW( MuscleCannotEquilibrate,
                      muscle->computeInitialFiberEquilibrium(state) );
    }

    // Test exception handling when invalid properties are propagated to
    // MuscleFixedWidthPennationModel and MuscleFirstOrderActivationDynamicModel
    // subcomponents.
    {
        Model model;
        auto muscle = new Thelen2003Muscle("muscle", 1., 0.5, 0.5, 0.);
        muscle->addNewPathPoint("p1", model.updGround(), SimTK::Vec3(0));
        muscle->addNewPathPoint("p2", model.updGround(), SimTK::Vec3(0,0,1));
        model.addForce(muscle);
        model.finalizeFromProperties();

        // Set each property that is propagated to the pennation model outside
        // its valid range.
        muscle->setOptimalFiberLength(0.);
        ASSERT_THROW(InvalidPropertyValue, model.finalizeFromProperties());
        muscle->setOptimalFiberLength(0.5);
        model.finalizeFromProperties();

        muscle->setPennationAngleAtOptimalFiberLength(SimTK::Pi/2.0 + 0.1);
        ASSERT_THROW(InvalidPropertyValue, model.finalizeFromProperties());
        muscle->setPennationAngleAtOptimalFiberLength(0.);
        model.finalizeFromProperties();

        muscle->setMaximumPennationAngle(SimTK::Pi/2.0 + 0.1);
        ASSERT_THROW(InvalidPropertyValue, model.finalizeFromProperties());
        muscle->setMaximumPennationAngle(0.);
        model.finalizeFromProperties();

        // Set each property that is propagated to the activation dynamics model
        // outside its valid range.
        muscle->setActivationTimeConstant(0.);
        ASSERT_THROW(InvalidPropertyValue, model.finalizeFromProperties());
        muscle->setActivationTimeConstant(0.1);
        model.finalizeFromProperties();

        muscle->setDeactivationTimeConstant(0.);
        ASSERT_THROW(InvalidPropertyValue, model.finalizeFromProperties());
        muscle->setDeactivationTimeConstant(0.1);
        model.finalizeFromProperties();

        muscle->setMinimumActivation(-0.1);
        ASSERT_THROW(InvalidPropertyValue, model.finalizeFromProperties());
        muscle->setMinimumActivation(0.01);
        model.finalizeFromProperties();
    }
}


TEST_CASE("testMillard2012EquilibriumMuscle")
{
    Millard2012EquilibriumMuscle muscle("muscle",
                            MaxIsometricForce0,
                            OptimalFiberLength0,
                            TendonSlackLength0,
                            PennationAngle0);

    muscle.setActivationTimeConstant(Activation0);
    muscle.setDeactivationTimeConstant(Deactivation0);

    double x0 = 0;
    double act0 = 0.2;

    Constant control(0.5);

    Sine motion(2.0*OptimalFiberLength0, 2*SimTK::Pi, 0);

    simulateMuscle(muscle, 
        x0, 
        act0, 
        &motion, 
        &control,
        false);

    // Test extremely short fiber where muscle should still initialize.
    {
        Model model;

        const double optimalFiberLength = 0.01; //short fiber and
        const double tendonSlackLength  = 100.0; //long tendon
        auto muscle = new Millard2012EquilibriumMuscle("muscle", 1.,
                          optimalFiberLength, tendonSlackLength, 0.);
        muscle->addNewPathPoint("p1", model.updGround(), SimTK::Vec3(0));
        muscle->addNewPathPoint("p2", model.updGround(), SimTK::Vec3(0,0,1));
        model.addForce(muscle);

        SimTK::State& state = model.initSystem();
        muscle->setActivation(state, 1.);
        model.realizeVelocity(state);
        muscle->computeInitialFiberEquilibrium(state);
    }

    // Test extremely short (but stretched) fiber, which should still initialize.
    {
        Model model;

        const double optimalFiberLength = 0.01; //short fiber and
        const double tendonSlackLength =  0.1; //short tendon
        auto muscle = new Millard2012EquilibriumMuscle("muscle", 1.,
            optimalFiberLength, tendonSlackLength, 0.);
        muscle->addNewPathPoint("p1", model.updGround(), SimTK::Vec3(0));
        muscle->addNewPathPoint("p2", model.updGround(), SimTK::Vec3(0, 0, 1));
        model.addForce(muscle);

        SimTK::State& state = model.initSystem();
        muscle->setActivation(state, 0.01);
        model.realizeVelocity(state);
        muscle->computeInitialFiberEquilibrium(state);
    }

    // Test exception handling when invalid properties are propagated to
    // MuscleFixedWidthPennationModel and MuscleFirstOrderActivationDynamicModel
    // subcomponents.
    {
        Model model;
        auto muscle = new Millard2012EquilibriumMuscle("mcl", 1., 0.5, 0.5, 0.);
        muscle->addNewPathPoint("p1", model.updGround(), SimTK::Vec3(0));
        muscle->addNewPathPoint("p2", model.updGround(), SimTK::Vec3(0,0,1));
        model.addForce(muscle);
        model.finalizeFromProperties();

        // Set each property that is propagated to the pennation model outside
        // its valid range.
        muscle->setOptimalFiberLength(0.);
        ASSERT_THROW(InvalidPropertyValue, model.finalizeFromProperties());
        muscle->setOptimalFiberLength(0.5);
        model.finalizeFromProperties();

        muscle->setPennationAngleAtOptimalFiberLength(SimTK::Pi/2.0 + 0.1);
        ASSERT_THROW(InvalidPropertyValue, model.finalizeFromProperties());
        muscle->setPennationAngleAtOptimalFiberLength(0.);
        model.finalizeFromProperties();

        muscle->set_maximum_pennation_angle(SimTK::Pi/2.0 + 0.1);
        ASSERT_THROW(InvalidPropertyValue, model.finalizeFromProperties());
        muscle->set_maximum_pennation_angle(0.);
        model.finalizeFromProperties();

        // Set each property that is propagated to the activation dynamics model
        // outside its valid range.
        muscle->setActivationTimeConstant(0.);
        ASSERT_THROW(InvalidPropertyValue, model.finalizeFromProperties());
        muscle->setActivationTimeConstant(0.1);
        model.finalizeFromProperties();

        muscle->setDeactivationTimeConstant(0.);
        ASSERT_THROW(InvalidPropertyValue, model.finalizeFromProperties());
        muscle->setDeactivationTimeConstant(0.1);
        model.finalizeFromProperties();

        muscle->setMinimumActivation(-0.1);
        ASSERT_THROW(InvalidPropertyValue, model.finalizeFromProperties());
        muscle->setMinimumActivation(0.01);
        model.finalizeFromProperties();
    }
}

TEST_CASE("testMillard2012AccelerationMuscle")
{
    Millard2012AccelerationMuscle muscle("muscle",
                            MaxIsometricForce0,
                            OptimalFiberLength0,
                            TendonSlackLength0,
                            PennationAngle0);

    //otherwise the simulations are a bit slow ...
    muscle.setMass(muscle.getMass()*10);

    MuscleFirstOrderActivationDynamicModel actMdl = muscle.getActivationModel();
    actMdl.set_activation_time_constant(Activation0);
    actMdl.set_deactivation_time_constant(Deactivation0);
    muscle.setActivationModel(actMdl);

    double x0 = 0;
    double act0 = 0.2;

    Constant control(0.5);

    Sine motion(0.1, SimTK::Pi, 0);

    // Note that simulateMuscle will call testMuscleEquilibriumSolve() but it
    // will ignore the Millard2012AccelerationMuscle because it is not an
    // equilibrium-based muscle model.
    simulateMuscle(muscle, 
        x0, 
        act0, 
        &motion, 
        &control,
        false);
}

TEST_CASE("testDeGrooteFregly2016Muscle") {

    DeGrooteFregly2016Muscle muscle;
    muscle.setName("muscle");
    muscle.set_max_isometric_force(MaxIsometricForce0);
    muscle.set_optimal_fiber_length(OptimalFiberLength0);
    muscle.set_tendon_slack_length(TendonSlackLength0);
    muscle.set_pennation_angle_at_optimal(PennationAngle0);
    muscle.set_tendon_compliance_dynamics_mode("explicit");
    muscle.set_activation_time_constant(Activation0);
    muscle.set_deactivation_time_constant(Deactivation0);

    double x0 = 0;
    double act0 = 0.2;

    Constant control(0.5);

    Sine motion(OptimalFiberLength0, SimTK::Pi, 0);

    simulateMuscle(muscle, 
        x0, 
        act0, 
        &motion,  
        &control, 
        false);
}

/*TEST_CASE("testThelen2003Muscle_Deprecated")
{
    Thelen2003Muscle_Deprecated muscle("muscle",
                                        MaxIsometricForce0,
                                        OptimalFiberLength0,
                                        TendonSlackLength0,
                                        PennationAngle0);

    muscle.setActivationTimeConstant(Activation0);
    muscle.setDeactivationTimeConstant(Deactivation0);

    double x0 = 0;
    double act0 = 0.2;

    Constant control(0.5);

    Sine motion(0.1, SimTK::Pi, 0);

    simulateMuscle( muscle,
                    x0,
                    act0,
                    NULL,
                    &control,
                    false);
}*/

/*TEST_CASE("testSchutte1993Muscle")
{
    Schutte1993Muscle_Deprecated muscle("muscle",
                                        MaxIsometricForce0,
                                        OptimalFiberLength0,
                                        TendonSlackLength0,
                                        PennationAngle0);

    muscle.setActivation1(ShutteDelpActivation1);
    muscle.setActivation2(ShutteDelpActivation2);

    double x0 = 0;
    double act0 = 0.2;

    Constant control(0.5);

    Sine motion(0.1, SimTK::Pi, 0);

    simulateMuscle(muscle, 
        x0, 
        act0, 
        &motion, 
        &control,
        false);

}*/

/*TEST_CASE("testDelp1990Muscle")
{
    Delp1990Muscle_Deprecated muscle("muscle",
                                    MaxIsometricForce0,
                                    OptimalFiberLength0,
                                    TendonSlackLength0,
                                    PennationAngle0);

    muscle.setActivation1(ShutteDelpActivation1);
    muscle.setActivation2(ShutteDelpActivation2);
    muscle.setMass(0.1);

    double x0 = 0;
    double act0 = 0.2;

    Constant control(0.5);

    Sine motion(0.1, SimTK::Pi, 0);

    simulateMuscle(muscle, 
        x0, 
        act0, 
        &motion, 
        &control,
        false);
}*/



